#include "main.h"
#include "can.h"
#include <string.h>
#include "bsp_can_motor_simulator.h"

/* Private variables ---------------------------------------------------------*/
static Motor_Feedback_t motor_feedback;

/* Private function prototypes -----------------------------------------------*/
static void update_motor_feedback(int16_t current_set);

/* Public functions ----------------------------------------------------------*/

/**
  * @brief  初始化电机模拟器
  * @retval None
  */
void motor_simulator_init(void)
{
    memset(&motor_feedback, 0, sizeof(motor_feedback));
    motor_feedback.temperature = 25;
    
    HAL_CAN_Start(&hcan);
    HAL_CAN_ActivateNotification(&hcan, CAN_IT_RX_FIFO0_MSG_PENDING);
}

/**
  * @brief  发送当前电机状态
  * @retval None
  */
void send_motor_status(void)
{
    CAN_TxHeaderTypeDef tx_header = {
        .StdId = MOTOR_ID, 
        .ExtId = 0, 
        .IDE = CAN_ID_STD, 
        .RTR = CAN_RTR_DATA, 
        .DLC = 8,
        .TransmitGlobalTime = DISABLE
    };
    
    uint8_t tx_data[8];
    memcpy(tx_data, &motor_feedback, 7); // 直接内存拷贝
    tx_data[7] = 0;
    
    HAL_CAN_AddTxMessage(&hcan, &tx_header, tx_data, NULL);
}

/**
  * @brief  手动发送指定参数的电机状态
  * @param  angle 角度值 (0-8191)
  * @param  speed_rpm 转速值
  * @param  current 电流值  
  * @param  temperature 温度值
  * @retval None
  */
void send_motor_status_manual(uint16_t angle, int16_t speed_rpm, int16_t current, uint8_t temperature)
{
    // 更新电机状态
    motor_feedback.angle = angle % MOTOR_MAX_ANGLE;
    motor_feedback.speed_rpm = speed_rpm;
    motor_feedback.current = current;
    motor_feedback.temperature = temperature;
    
    // 发送状态
    send_motor_status();
}

/**
  * @brief  自动更新并发送电机状态（角度自动递增）
  * @retval None
  */
void send_motor_status_auto_update(void)
{
    // 自动更新角度
    motor_feedback.angle = (motor_feedback.angle + 50) % MOTOR_MAX_ANGLE;
    
    // 发送状态
    send_motor_status();
    
    // 翻转LED指示
    HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_8);
}

/**
  * @brief  CAN接收回调函数
  * @param  hcan CAN句柄指针
  * @retval None
  */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    CAN_RxHeaderTypeDef rx_header;
    uint8_t rx_data[8];
    
    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);

    if (rx_header.StdId >= MOTOR_CONTROL_ID_MIN && rx_header.StdId <= MOTOR_CONTROL_ID_MAX) {
        int16_t current_set = *(int16_t*)rx_data;
        
        update_motor_feedback(current_set);
        send_motor_status();
        
        // 翻转LED指示
        HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_8);
    }
}

/**
  * @brief  电机模拟器测试函数
  * @retval None
  */
void motor_simulator_test(void)
{
    static const uint8_t test_data[8] = {0x00, 0x10};
    
    CAN_RxHeaderTypeDef rx_header = {
        .StdId = MOTOR_CONTROL_ID_MIN,
        .DLC = 8
    };
    uint8_t rx_data[8] = {0};
    
    memcpy(rx_data, test_data, sizeof(test_data));
    
    // 模拟接收处理
    int16_t current_set = *(int16_t*)rx_data;
    update_motor_feedback(current_set);
    send_motor_status();
}

/* Getter functions ----------------------------------------------------------*/

uint16_t get_motor_angle(void)
{
    return motor_feedback.angle;
}

int16_t get_motor_speed(void)
{
    return motor_feedback.speed_rpm;
}

int16_t get_motor_current(void)
{
    return motor_feedback.current;
}

uint8_t get_motor_temperature(void)
{
    return motor_feedback.temperature;
}

/* Setter functions ----------------------------------------------------------*/

void set_motor_angle(uint16_t angle)
{
    motor_feedback.angle = angle % MOTOR_MAX_ANGLE;
}

void set_motor_speed(int16_t speed)
{
    motor_feedback.speed_rpm = speed;
}

void set_motor_current(int16_t current)
{
    motor_feedback.current = current;
}

void set_motor_temperature(uint8_t temp)
{
    motor_feedback.temperature = temp;
}

/* Private functions ---------------------------------------------------------*/

/**
  * @brief  更新电机反馈数据
  * @param  current_set 设置的电流值
  * @retval None
  */
static void update_motor_feedback(int16_t current_set)
{
    motor_feedback.angle = (motor_feedback.angle + 100) % MOTOR_MAX_ANGLE;
    motor_feedback.speed_rpm = current_set / 10;
    motor_feedback.current = current_set;
    motor_feedback.temperature = 30;
}